from typing import Callable
import pickle

import numpy as np
import heapq
from dora_utils import (
    distance_points,
    closest_vertex,
    interpolation,
    smoothness,
    transform_to_world,
    compute_orientation,
    Status,
    BorrowLane,
    Action,
    Decision,
)
from dora import DoraStatus
from scipy.spatial.transform import Rotation as R
from shapely.geometry import Polygon, Point

import carla
from carla import Client, LaneType, Location, Rotation, Vector3D, BoundingBox

DEBUG = False
DEBUG_PREDICTION = False
DEBUG_GT = True  # use CARLA ground truth
OVERSIZE_INT = 10000  # TODO: consider a more proper way to implement this

# Prediction
PREDICTION_TIME = 5
PREDICTION_STEP = 0.25
PREDICTION_VEL_AVERAGE_WINDOW = 5
VEHICLE_RADIUS = 0.5
BICYCLE_RADIUS = 0.5
PERSON_RADIUS = 0.5
OBSTACLE_HEIGHT = 2.0
OBSTACLE_CONFIDENCE_THRESHOLD = 0.4
EGO_HEIGHT = 1.476
EGO_HALF_LENGTH = 2.463 + 0.25
EGO_HALF_WIDTH = 1.058 + 0.25
EGO_VERTS = np.array(
    [
        [-EGO_HALF_LENGTH, -EGO_HALF_WIDTH, 0],  # Rear bottom left
        [-EGO_HALF_LENGTH, -EGO_HALF_WIDTH, EGO_HEIGHT],  # Rear top left
        [-EGO_HALF_LENGTH, EGO_HALF_WIDTH, 0],  # Rear bottom right
        [-EGO_HALF_LENGTH, EGO_HALF_WIDTH, EGO_HEIGHT],  # Rear top right
        [EGO_HALF_LENGTH, -EGO_HALF_WIDTH, 0],  # Front bottom left
        [EGO_HALF_LENGTH, -EGO_HALF_WIDTH, EGO_HEIGHT],  # Front top left
        [EGO_HALF_LENGTH, EGO_HALF_WIDTH, 0],  # Front bottom right
        [EGO_HALF_LENGTH, EGO_HALF_WIDTH, EGO_HEIGHT],  # Front top right
    ]
)

# Traffic Light
STOP_DIS_TO_INTERSECTION = 0.5

# Planning general
CRUISE_SPEED = 6
TURNING_SPEED = 4
TURNING_THRESHOLD_DEG = 30
TURNING_WAYPOINTS_CHECK_WINDOW_SIZE = 10
SPEED_LIMIT = 11.1
LON_ACC = 6
LAT_ACC = 2.3
VERT_ACC = 0.15
LON_JERK = 5
LAT_JERK = 15
LANE_BLOCKING_LAT_DIS_FROM_REF = 2
LANE_BORROW_ENABLE_COUNTER = 20
LANE_BORROW_DISABLE_COUNTER = 30
LANE_BORROW_EGO_SPEED_THRESHOLD = 5.0
LANE_BORROW_OBSTACLE_SPEED_THRESHOLD = 3.5
LANE_BORROW_BLOCKING_CHECKING_DISTANCE = 20
LANE_BORROW_LON_SAFE_TIME = 5
LANE_BORROW_LON_SAFE_DIS = 15
LANE_BORROW_INTERSECTION_CHECK = False
LANE_BORROW_DISABLE_WITHIN_DIS_TO_INTERSECTION = 10
LANE_BORROW_DISABLE_WITHIN_DIS_TO_DESTINATION = 30
LANE_BORROW_DEVIATION_REPLAN_THRESHOLD = 0.8
LANE_BORROW_VEL_DIFF = 3
SMOOTH_ROUTE = False
EMERGENCY_STOP_TIME = 2
EMERGENCY_STOP_DIS = 3
STOP_KEEPING_THRESHOLD = 5

if DEBUG_GT:
    client = Client("localhost", 2000)
    client.set_timeout(2.0)

    TWO_WHEELED_VEHICLE_BBOX = {
        "vehicle.harley-davidson.low_rider": BoundingBox(
            Location(x=-0.008867, y=0.002227, z=0.659915),
            Vector3D(x=1.177870, y=0.381839, z=0.638285),
        ),
        "vehicle.vespa.zx125": BoundingBox(
            Location(x=0.010469, y=0.000000, z=0.621152),
            Vector3D(x=0.902333, y=0.427844, z=0.617814),
        ),
        "vehicle.diamondback.century": BoundingBox(
            Location(x=0.000020, y=0.000000, z=0.525391),
            Vector3D(x=0.821422, y=0.186258, z=0.511951),
        ),
        "vehicle.gazelle.omafiets": BoundingBox(
            Location(x=-0.015645, y=0.006426, z=0.580164),
            Vector3D(x=0.917720, y=0.164464, z=0.562829),
        ),
        "vehicle.bh.crossbike": BoundingBox(
            Location(x=0.000293, y=-0.000508, z=0.541191),
            Vector3D(x=0.743644, y=0.429629, z=0.539789),
        ),
        "vehicle.kawasaki.ninja": BoundingBox(
            Location(x=-0.019609, y=0.000371, z=0.585439),
            Vector3D(x=1.016676, y=0.401290, z=0.572727),
        ),
        "vehicle.yamaha.yzf": BoundingBox(
            Location(x=-0.004258, y=0.000000, z=0.649841),
            Vector3D(x=1.104723, y=0.433517, z=0.625573),
        ),
    }

class Obstacle:
    def __init__(self, id, loc, vel, type_id=None, bbox=None, transform=None) -> None:
        self.id = id
        if DEBUG_GT:
            self.loc = np.array([loc.x, loc.y, loc.z])
            self.vel = np.array([vel.x, vel.y, vel.z])
        else:
            self.loc = np.array(loc)
            self.vel = np.array([vel[0], vel[1], 0.0]) # Only predict velocity in 2D
        self.type_id = type_id
        self.bbox = bbox
        self.transform = transform
        if self.bbox is None:
            if self.id == 0.0:
                self.radius = PERSON_RADIUS
            elif self.id == 1.0 or self.id == 3.0:
                self.radius = BICYCLE_RADIUS
            else:
                self.radius = VEHICLE_RADIUS
        else:
            self.radius = VEHICLE_RADIUS

    def get_vertices(self):
        # Return vertices coordinates in world frame
        # there is a bug with 2D wheeled vehicle bbox in carla, so we have to check it manually
        if self.bbox is None:
            x, y, z = self.loc
            vertices = np.array([[x + self.radius*np.cos(angle_rad), y + self.radius*np.sin(angle_rad), z] for angle_rad in np.arange(0, 2 * np.pi, np.pi / 30)])
        else:
            if self.type_id in TWO_WHEELED_VEHICLE_BBOX:
                verts = TWO_WHEELED_VEHICLE_BBOX[self.type_id].get_world_vertices(
                    self.transform
                )
            else:
                verts = self.bbox.get_world_vertices(self.transform)

            vertices = np.array([[vert.x, vert.y, vert.z] for vert in verts])
        return vertices

class DecisionMaker:
    def __init__(self, hd_map, gps_waypoints, goal_location) -> None:
        self.hd_map = hd_map
        self.goal_location = goal_location
        self.cur_position = np.array([])
        self.cur_speed = np.array([])
        self.gps_waypoints = gps_waypoints[:, :3]
        self.obstacles = np.array([])  # obstacles from nearer to further
        self.decision = Decision(Status.CRUISE, BorrowLane.NONE, Action.NONE)
        self.counter_from_stop = 100
        self.borrow_lane_ref_line_wpts = np.array(
            []
        )  # Contains the ref line wpts of adjacent lane checking area (5 + 20 + 5*2)
        self.waypoints = gps_waypoints[:, :3]
        self.target_speeds = np.array(
            [CRUISE_SPEED for _ in range(len(self.waypoints))]
        )
        self.local_waypoints = np.array([])
        self.self_lane_wpt = None  # Centerline waypoint of the closest gps waypoints
        self.cur_lane_wpt = None  # Centerline waypoint of the current position
        self.left_lane_wpt = None  # left according to the self lane
        self.right_lane_wpt = None  # right according to the self lane
        self.obstacle_blocking_counter = 0
        self.able_to_use_self_route_counter = 0
        self.obstacle_prediction = np.array([])  # TODO: not implemented
        self.obstacles_to_yield = list()
        self.traffic_light = None
        self.obstacle_blocking = None  # The obstacle that blocks the route
        self.obstacle_blocking_wpts = np.array(
            []
        )  # Reference Wpts of the blocking area
        self.use_obstacle_blocking_wpts = False

    @staticmethod
    def angle_diffs(v1, v2):
        dot_product = np.dot(v1, v2)
        mag1 = np.linalg.norm(v1)
        mag2 = np.linalg.norm(v2)
        if mag2 < 0.1 or mag1 < 0.1:
            return 0
        angle_rad = np.arccos(dot_product / (mag1 * mag2))
        return abs(np.rad2deg(angle_rad))

    @staticmethod
    def filter_consecutive_duplicate(x):
        return np.array([elem for i, elem in enumerate(x) if (elem - x[i - 1]).any() or i == 0])

    @staticmethod
    def is_collide(ego_loc, yaw, obstacle_loc, obstacle_radius):
        ego_z = ego_loc[2]
        obstacle_z = obstacle_loc[2]
        bottom_face_idx = [0, 2, 4, 6, 0]
        ego_bbox_xy_local = EGO_VERTS[bottom_face_idx, :2]
        ego_bbox_xy = transform_to_world(ego_bbox_xy_local, ego_loc, yaw)
        if (
            -OBSTACLE_HEIGHT <= obstacle_z - ego_z <= EGO_HEIGHT
            and Point(obstacle_loc[:2]).distance(Polygon(ego_bbox_xy))
            <= obstacle_radius
        ):
            return True
        return False

    def check_blocking_route(
        self, forward_lane_ref_line_xyz, update_blocking_info=True
    ):
        # Check if any obstacle is on the way
        for obstacle in self.obstacles:
            obstacle_blocking_wpts = list()
            for (
                vertice
            ) in (
                obstacle.get_vertices()
            ):
                vert_on_ref_wpt = self.hd_map.get_waypoint(
                    Location(float(vertice[0]), float(vertice[1]), float(vertice[2])),
                    project_to_road=True,
                    lane_type=LaneType.Driving,
                )
                vert_on_ref_xyz = np.array(
                    [
                        vert_on_ref_wpt.transform.location.x,
                        vert_on_ref_wpt.transform.location.y,
                        vert_on_ref_wpt.transform.location.z,
                    ]
                )
                # Obstacle vertices projected centerline waypoints within the range of the route centerline waypoints
                distance_to_route = np.linalg.norm(
                    forward_lane_ref_line_xyz - vert_on_ref_xyz, axis=1
                )
                # vertice projecting to forward reference line, latitude distance within 2 meters
                # TODO: Will the obstacle large enough to cross both sides of the reference line?
                distance_to_vertice = np.linalg.norm(vert_on_ref_xyz - vertice)

                if (
                    np.any(distance_to_route <= 0.5)
                    and distance_to_vertice < LANE_BLOCKING_LAT_DIS_FROM_REF
                ):
                    obstacle_blocking_wpts.append(vert_on_ref_wpt)

            if obstacle_blocking_wpts:
                if DEBUG:
                    print(f"Obstacle {obstacle.id} is blocking the {'lane borrow' if not update_blocking_info else 'self'} route!")
                if update_blocking_info:
                    self.obstacle_blocking = obstacle
                    # Sort according to its s value of the reference line
                    self.obstacle_blocking_wpts = np.array(
                        sorted(obstacle_blocking_wpts, key=lambda wpt: wpt.s)
                    )
                return True
            else:
                if DEBUG:
                    print(f"Obstacle {obstacle.id} is not blocking the {'lane borrow' if not update_blocking_info else 'self'} route!")

        if update_blocking_info:
            self.obstacle_blocking = None
            self.obstacle_blocking_wpts = np.array([])

        return False

    def on_route(self):
        if self.cur_lane_wpt is None or self.self_lane_wpt is None:
            return False

        if (
            self.cur_lane_wpt.road_id == self.self_lane_wpt.road_id
            and self.cur_lane_wpt.lane_id == self.self_lane_wpt.lane_id
        ):
            return True
        return False

    def check_lane_borrow_conditions(self):
        if self.obstacle_blocking is None:
            if DEBUG:
                print(f"Lane borrow condition checking, no blocking obstacle found!")
            return False

        if LANE_BORROW_INTERSECTION_CHECK:
            # Not within a distance to junctions
            dis_to_intersection = self.distance_to_intersection(self.self_lane_wpt)
            if (
                dis_to_intersection is not None
                and dis_to_intersection < LANE_BORROW_DISABLE_WITHIN_DIS_TO_INTERSECTION
            ):
                if DEBUG:
                    print(
                        f"Lane borrow checking, within {LANE_BORROW_DISABLE_WITHIN_DIS_TO_INTERSECTION} to intersection!"
                    )
                return False

        if (
            np.linalg.norm(self.cur_position - self.goal_location)
            <= LANE_BORROW_DISABLE_WITHIN_DIS_TO_DESTINATION
        ):
            if DEBUG:
                print(
                    f"Lane borrow disabled, within {LANE_BORROW_DISABLE_WITHIN_DIS_TO_DESTINATION} m to the destination!"
                )
            return False

        if self.obstacle_blocking_counter >= LANE_BORROW_ENABLE_COUNTER:
            if np.linalg.norm(self.cur_speed) >= LANE_BORROW_EGO_SPEED_THRESHOLD:
                if DEBUG:
                    print(f"Ego speed too high to lane borrow!")
                return False
            if (
                np.linalg.norm(self.obstacle_blocking.vel)
                >= LANE_BORROW_OBSTACLE_SPEED_THRESHOLD
            ):
                if DEBUG:
                    print(
                        f"Blocking obstacle speed {np.linalg.norm(self.obstacle_blocking.vel)} too high to lane borrow!"
                    )
                return False
            if (
                self.angle_diffs(self.cur_speed[:2], self.obstacle_blocking.vel[:2])
                > 30
            ):
                if DEBUG:
                    print(f"Blocking obstacle crossing the street, don't lane borrow!")
                return False
            if DEBUG:
                print(f"Lane borrow checking succeed! Start lane borrow!")
            return True

        if DEBUG:
            print(f"Lane borrow counter not enough yet! Waiting for lane borrow!")
        return False

    def __is_intersection(self, waypoint) -> bool:
        if waypoint.is_junction:
            return True
        if hasattr(waypoint, "is_intersection"):
            return waypoint.is_intersection
        return False

    def distance_to_intersection(self, waypoint, max_distance_to_check: float = 30):
        """Computes the distance (in meters) from location to an intersection.

        The method starts from location, moves forward until it reaches an
        intersection or exceeds max_distance_to_check.

        Args:
            location (:py:class:`~pylot.utils.Location`): The starting location
                in world coordinates.
            max_distance_to_check (:obj:`int`): Max distance to move forward
                 (in meters).

        Returns:
            :obj:`int`: The distance in meters, or None if there is no
            intersection within max_distance_to_check.
        """
        if not waypoint:
            return None
        # We're already in an intersection.
        if self.__is_intersection(waypoint):
            return 0
        for i in range(1, max_distance_to_check + 1):
            waypoints = waypoint.next(1)
            if not waypoints or len(waypoints) == 0:
                return None
            for w in waypoints:
                if self.__is_intersection(w):
                    return i
            waypoint = waypoints[0]
        return None

    def adjacent_lane_checking_ref_wpts(self, lane_wpt):
        if lane_wpt is None:
            if DEBUG:
                print(f"Lane borrow, adjacent lane checking, no lane waypoint!")
            return None

        # if lane_wpt.road_id != self.self_lane_wpt.road_id:
        #     if DEBUG:
        #         print(f"Lane borrow, adjacent lane checking, not in the same road! {lane_wpt.road_id} vs {self.self_lane_wpt.road_id}")
        #     return None

        is_in_same_direction = np.sign(lane_wpt.lane_id) == np.sign(
            self.self_lane_wpt.lane_id
        )

        if is_in_same_direction:
            forward_ref_wpts = lane_wpt.next_until_lane_end(1.0)
        else:
            forward_ref_wpts = lane_wpt.previous_until_lane_start(1.0)

        backward_ref_wpts = list()
        wpt = lane_wpt
        for _ in range(LANE_BORROW_LON_SAFE_DIS + 1):
            if is_in_same_direction:
                wpt = wpt.previous(1.0)
            else:
                wpt = wpt.next(1.0)
            if not wpt or len(wpt) == 0:
                break
            wpt = wpt[0]
            backward_ref_wpts.append(wpt)
        adjacent_lane_checking_ref_wpts = (
            backward_ref_wpts[::-1]
            + forward_ref_wpts[
                : LANE_BORROW_BLOCKING_CHECKING_DISTANCE + LANE_BORROW_LON_SAFE_DIS * 2
            ]
        )
        return np.array(
            [
                [
                    wpt.transform.location.x,
                    wpt.transform.location.y,
                    wpt.transform.location.z,
                ]
                for wpt in adjacent_lane_checking_ref_wpts
            ]
        )

    def start_lane_borrow(self):
        if DEBUG:
            print(f"Lane borrow, start lane borrow!")
        # if self.decision.action == Action.YIELD: # Don't lane borrow while yielding
        #     if DEBUG:
        #         print(f"Yielding, stop lane borrow!")
        #     return
        if self.decision.status == Status.CRUISE:
            if DEBUG:
                print(f"Cruise, change to lane borrow.")
            self.decision.status = Status.LANE_BORROW
        if self.left_lane_wpt is None and self.right_lane_wpt is None:
            if DEBUG:
                print(f"Both left and right lane are None! Try to find them from blocking wpt.")
            self.left_lane_wpt = self.obstacle_blocking_wpts[0].get_left_lane()
            self.right_lane_wpt = self.obstacle_blocking_wpts[0].get_right_lane()
            self.use_obstacle_blocking_wpts = True
        else:
            self.use_obstacle_blocking_wpts = False

        left_lane_ref = self.adjacent_lane_checking_ref_wpts(self.left_lane_wpt)
        right_lane_ref = self.adjacent_lane_checking_ref_wpts(self.right_lane_wpt)
        if DEBUG:
            print(f"Left lane ref length is {len(left_lane_ref)}")
            print(f"Right lane ref length is {len(right_lane_ref)}.")
        left_blocking, right_blocking = True, True
        if left_lane_ref is not None and len(left_lane_ref) > 0:
            if DEBUG:
                print(f"Left lane reference line find!")
            left_blocking = self.check_blocking_route(
                left_lane_ref, update_blocking_info=False
            )
        if right_lane_ref is not None and len(right_lane_ref) > 0:
            if DEBUG:
                print(f"Right lane reference line find!")
            right_blocking = self.check_blocking_route(
                right_lane_ref, update_blocking_info=False
            )

        # self.borrow_lane_ref_line_wpts = np.array([])
        if left_blocking and right_blocking:
            if DEBUG:
                print(f"Both left and right lane are blocking!")
            self.decision.status = Status.CRUISE
            self.decision.borrow_lane = BorrowLane.NONE
            return
        if not left_blocking:
            if (
                self.self_lane_wpt.lane_change == carla.LaneChange.Both
                or self.self_lane_wpt.lane_change == carla.LaneChange.Left
            ):  # First check if left lane changable
                if self.decision.borrow_lane == BorrowLane.RIGHT:
                    if DEBUG:
                        print(f"Borrowing right lane, do not try to borrow the left one.")
                    return
                if DEBUG:
                    print(f"Both side crossable, cross left one.")
                self.decision.borrow_lane = BorrowLane.LEFT
                self.borrow_lane_ref_line_wpts = np.asarray(left_lane_ref)
                return
        if not right_blocking:
            if (
                self.self_lane_wpt.lane_change == carla.LaneChange.Both
                or self.self_lane_wpt.lane_change == carla.LaneChange.Right
            ):  # Then check if right lane changeable
                if self.decision.borrow_lane == BorrowLane.LEFT:
                    if DEBUG:
                        print(f"Borrowing left lane, do not try to borrow the right one.")
                    return
                if DEBUG:
                    print(f"Right lane not blocking, cross right lane.")
                self.decision.borrow_lane = BorrowLane.RIGHT
                self.borrow_lane_ref_line_wpts = np.asarray(right_lane_ref)
                return
        if not left_blocking and (
            self.self_lane_wpt.left_lane_marking.type == carla.LaneMarkingType.Broken
            or self.self_lane_wpt.left_lane_marking.type
            in [carla.LaneMarkingType.BottsDots, carla.LaneMarkingType.BrokenBroken]
        ):  # Cross to the left direction lane, possibly opposite direction
            if DEBUG:
                print(f"Cross broken center line to the left lane.")
            self.decision.borrow_lane = BorrowLane.LEFT
            self.borrow_lane_ref_line_wpts = np.array(left_lane_ref)
            return

        self.decision.status = Status.CRUISE
        self.decision.borrow_lane = BorrowLane.NONE
        if DEBUG:
            print(f"None of the lane marking crossable!")
        return

    def end_lane_borrow(self):
        if DEBUG:
            print(f"Lane borrow, end lane borrow!")
        # Let local planner deal with the lane change
        if self.decision.status == Status.LANE_BORROW:
            self.decision.status = Status.CRUISE
            self.decision.borrow_lane = BorrowLane.NONE
            self.borrow_lane_ref_line_wpts = np.array([])
        return

    def plan_full_lane_borrow_path(self):
        # Find lane borrow start point
        lane_borrow_begin_lon_dis = max(
            LANE_BORROW_LON_SAFE_DIS,
            LANE_BORROW_LON_SAFE_TIME * np.linalg.norm(self.cur_speed),
        )
        blocking_start = self.obstacle_blocking_wpts[0]
        start_borrow_pt = blocking_start.previous(lane_borrow_begin_lon_dis)
        if not start_borrow_pt or len(start_borrow_pt) == 0:
            if DEBUG:
                print(f"Cannot find lane borrow beginning point!")
            return None
        start_borrow_pt = start_borrow_pt[0]
        start_borrow_pt_xyz = np.array(
            [
                start_borrow_pt.transform.location.x,
                start_borrow_pt.transform.location.y,
                start_borrow_pt.transform.location.z,
            ]
        )
        # Find lane borrow end point
        lane_borrow_end_lon_dis = max(
            LANE_BORROW_LON_SAFE_DIS,
            LANE_BORROW_LON_SAFE_TIME * np.linalg.norm(self.cur_speed),
        )
        blocking_end = self.obstacle_blocking_wpts[-1]
        end_borrow_pt = blocking_end.next(lane_borrow_end_lon_dis)
        if not end_borrow_pt or len(end_borrow_pt) == 0:
            if DEBUG:
                print(f"Cannot find lane borrow end point!")
            return None
        end_borrow_pt = end_borrow_pt[0]
        end_borrow_pt_xyz = np.array(
            [
                end_borrow_pt.transform.location.x,
                end_borrow_pt.transform.location.y,
                end_borrow_pt.transform.location.z,
            ]
        )

        # Borrowed lane start point
        start_mid_wpt_xyz = (
            start_borrow_pt_xyz
            + np.array(
                [
                    blocking_start.transform.location.x,
                    blocking_start.transform.location.y,
                    blocking_start.transform.location.z,
                ]
            )
        ) / 2
        borrow_lane_start_index, borrowed_lane_start_xyz = closest_vertex(
            self.borrow_lane_ref_line_wpts, np.array([start_mid_wpt_xyz])
        )

        # Borrowed lane end point
        end_mid_wpt_xyz = (
            end_borrow_pt_xyz
            + np.array(
                [
                    blocking_end.transform.location.x,
                    blocking_end.transform.location.y,
                    blocking_end.transform.location.z,
                ]
            )
        ) / 2
        borrow_lane_end_index, borrowed_lane_end_xyz = closest_vertex(
            self.borrow_lane_ref_line_wpts, np.array([end_mid_wpt_xyz])
        )

        # Find gps waypoints before start borrow pt, if fewer than 5 pts, add 5 meters
        before_index, start_loc = closest_vertex(
            self.gps_waypoints, np.array([start_borrow_pt_xyz])
        )
        waypoints_before_lane_borrow = self.gps_waypoints[
            :before_index, :
        ]  # retrieve to the previous point, if at the first point, it could be an empty one
        if len(waypoints_before_lane_borrow) < 5:
            waypoints_before_lane_borrow = list()
            _wpt = start_borrow_pt
            for _ in range(5):
                _wpt = _wpt.previous(1.0)
                if not _wpt or len(_wpt) == 0:
                    break
                _wpt = _wpt[0]
                waypoints_before_lane_borrow.append(
                    [
                        _wpt.transform.location.x,
                        _wpt.transform.location.y,
                        _wpt.transform.location.z,
                    ]
                )
            waypoints_before_lane_borrow.reverse()
            waypoints_before_lane_borrow = np.array(waypoints_before_lane_borrow)

        # Find gps waypoints after end borrow pt
        after_index, _ = closest_vertex(
            self.gps_waypoints, np.array([end_borrow_pt_xyz])
        )
        waypoints_after_lane_borrow = self.gps_waypoints[
            after_index + 1 :, :
        ]  # retrieve to the next point, if at the last point, it could be an empty one
        if len(waypoints_after_lane_borrow) < 5:
            waypoints_after_lane_borrow = list()
            _wpt = end_borrow_pt
            for _ in range(5):
                _wpt = _wpt.next(1.0)
                if not _wpt or len(_wpt) == 0:
                    break
                _wpt = _wpt[0]
                waypoints_after_lane_borrow.append(
                    [
                        _wpt.transform.location.x,
                        _wpt.transform.location.y,
                        _wpt.transform.location.z,
                    ]
                )
            waypoints_after_lane_borrow = np.array(waypoints_after_lane_borrow)

        # Interpolate the lane borrow waypoints with 1 meter
        lane_borrow_waypoints = smoothness(
            np.array(
                [
                    start_borrow_pt_xyz,
                    borrowed_lane_start_xyz,
                    borrowed_lane_end_xyz,
                    end_borrow_pt_xyz,
                ]
            )
        )

        # Concatenate them together
        full_lane_borrow_path = np.concatenate(
            (
                waypoints_before_lane_borrow,
                lane_borrow_waypoints,
                waypoints_after_lane_borrow,
            )
        )

        closest_index, closest_xyz = closest_vertex(
            full_lane_borrow_path, np.array([self.cur_position])
        )
        if (
            np.linalg.norm(closest_xyz - self.cur_position)
            >= LANE_BORROW_DEVIATION_REPLAN_THRESHOLD
            and not self.on_route()
        ):  # Deviate from original lane borrow path, replan
            closest_index_to_borrow_lane, _ = closest_vertex(
                self.borrow_lane_ref_line_wpts, np.array([self.cur_position])
            )
            if (
                closest_index_to_borrow_lane < borrow_lane_start_index
            ):  # if fall behind the borrow lane start location
                if DEBUG:
                    print(f"Replan lane borrow path, fall behind the borrow lane!")
                lane_borrow_waypoints = smoothness(
                    np.array(
                        [
                            borrowed_lane_start_xyz,
                            borrowed_lane_end_xyz,
                            end_borrow_pt_xyz,
                        ]
                    )
                )
                full_lane_borrow_path = np.concatenate(
                    (
                        self.borrow_lane_ref_line_wpts[
                            closest_index_to_borrow_lane:borrow_lane_start_index
                        ],
                        lane_borrow_waypoints,
                        waypoints_after_lane_borrow,
                    )
                )
            else:
                if DEBUG:
                    print(f"Replan not executed, already ahead of the borrow lane!")

        return full_lane_borrow_path

    def plan_lane_borrow_traj(self):
        if self.obstacle_blocking_wpts is None or len(self.obstacle_blocking_wpts) == 0:
            if DEBUG:
                print(f"No obstacle blocking, use the last waypoints.")
            waypoints = self.filter_consecutive_duplicate(self.waypoints.copy())
        else:
            if DEBUG:
                print(f"Obstacle blocking, plan the full lane borrow path.")
            lane_borrow_path = self.plan_full_lane_borrow_path()
            if lane_borrow_path is None:
                self.decision.status = Status.Cruise
                self.decision.borrow_lane = BorrowLane.NONE
                self.plan_cruise_traj()
                return
            waypoints = self.filter_consecutive_duplicate(lane_borrow_path)
            # Smoothness
            if SMOOTH_ROUTE:
                waypoints = smoothness(waypoints, dis=1.0, type="cubic")

        # Current global waypoints starts from the closest point
        assert waypoints.shape[0] > 1, f"No enough waypoints {waypoints} to plan!"
        assert waypoints.shape[1] == 3, "Waypoints should be in 3D!"
        closest_index, closest_xyz = closest_vertex(
            waypoints, np.array([self.cur_position])
        )
        self.waypoints = waypoints[min(closest_index, len(waypoints)-2):, :]

        # Set target speed
        if self.obstacle_blocking is None:
            target_speed = CRUISE_SPEED
        else:
            obstacle_speed = np.linalg.norm(self.obstacle_blocking.vel)
            target_speed = obstacle_speed + LANE_BORROW_VEL_DIFF
            if DEBUG:
                print(
                    f"Blocking obstacle speed is {obstacle_speed}, ego target speed is {target_speed}"
                )
        if self.turning_in_front():
            if DEBUG:
                print(f"SHARP turn in front, reduce the speed to {target_speed}.")
            target_speed = min(TURNING_SPEED, target_speed)
        self.target_speeds = np.linspace(
            np.linalg.norm(self.cur_speed), target_speed, len(self.waypoints)
        )

    def turning_in_front(self):
        if len(self.waypoints) <= 1:
            return False
        wpts_xy = self.waypoints[:, :2]
        # Calculate direction vectors
        directions = wpts_xy[1:] - wpts_xy[:-1]

        yaws = np.arctan2(directions[:, 1], directions[:, 0])
        diff_angles_deg = np.degrees(yaws - yaws[0])
        abs_diff_angles_deg = np.abs((diff_angles_deg + 180) % 360 - 180)

        # Compute the moving average using a window of size TURNING_WAYPOINTS_CHECK_WINDOW_SIZE
        cumsum = np.cumsum(abs_diff_angles_deg)
        cumsum = np.insert(cumsum, 0, 0)
        moving_avg = (
            cumsum[TURNING_WAYPOINTS_CHECK_WINDOW_SIZE:]
            - cumsum[:-TURNING_WAYPOINTS_CHECK_WINDOW_SIZE]
        ) / TURNING_WAYPOINTS_CHECK_WINDOW_SIZE

        return np.any(moving_avg > TURNING_THRESHOLD_DEG)

    def plan_cruise_traj(self):
        self.waypoints = self.gps_waypoints.copy()
        if self.turning_in_front():
            if DEBUG:
                print(f"SHARP turn in front, reduce the speed to {TURNING_SPEED}.")
            self.target_speeds = np.linspace(
                np.linalg.norm(self.cur_speed), TURNING_SPEED, len(self.waypoints)
            )
        else:
            self.target_speeds = np.linspace(
                np.linalg.norm(self.cur_speed), CRUISE_SPEED, len(self.waypoints)
            )

    def calculate_ego_locs(self, prediction_time, step=PREDICTION_STEP):
        ego_locs = list()
        assert self.local_waypoints.shape[0] > 1, "Local waypoints are not enough!"
        distances, x_interp, y_interp, z_interp = interpolation(
            self.local_waypoints, inter_type="linear"
        )
        # calculate predicted positions based on speed
        cur_dis = 0
        # TODO: not sure whether this is correct or not
        for _ in np.arange(0, prediction_time + step, step):
            cur_speed = np.interp(cur_dis, distances, self.local_waypoints[:, 3])
            cur_dis += cur_speed * step
            x_pred = x_interp(cur_dis)
            y_pred = y_interp(cur_dis)
            z_pred = z_interp(cur_dis)
            ego_locs.append([x_pred, y_pred, z_pred])
        return np.array(ego_locs)

    def calculate_obstacle_locs(self, obstacle, prediction_time, step=PREDICTION_STEP):
        init_loc, vel = obstacle.loc, obstacle.vel
        times = np.arange(0, prediction_time + step, step)
        displacements = np.outer(times, vel)
        return init_loc + displacements, obstacle.radius

    def collision_detection(self, ego_locs, obstacle_locs, obstacle_radius):
        # TODO: determine if collide at this locs. Return index and its location
        assert (
            ego_locs.shape[0] == obstacle_locs.shape[0]
        ), "ego locs and obstacle locs are not in the same length"
        for i in range(len(ego_locs)):
            if i < len(ego_locs) - 1:
                yaw = compute_orientation(ego_locs[i], ego_locs[i + 1])
            if self.is_collide(ego_locs[i], yaw, obstacle_locs[i], obstacle_radius):
                return i, ego_locs[i]
        return OVERSIZE_INT * 10, None

    def plan_yielding_traj(self):
        # Checking along local planning waypoints, including the traffic light; truncate the global planning waypoints
        # Check traffic light
        stop_at_junction = False
        if self.traffic_light is not None:
            if self.traffic_light.get_state() in [
                carla.TrafficLightState.Red,
                carla.TrafficLightState.Yellow,
            ]:
                stop_at_junction = True
                if DEBUG:
                    print(f"Traffic light red or yellow, prepare to stop!")

        # Predict for a certain amount of time
        ego_vehicle_locs = self.calculate_ego_locs(
            PREDICTION_TIME
        )  # np array of locations
        earliest_collision_index = OVERSIZE_INT * 10
        earliest_collision_loc = None
        for obstacle in self.obstacles:
            obstacle_locs, obstacle_radius = self.calculate_obstacle_locs(obstacle, PREDICTION_TIME)
            # If use the obstacle prediction, only "person", "bicycle", "car", "motorcycle", "airplane", "bus" are considered
            first_collision_index, first_collision_loc = self.collision_detection(
                ego_vehicle_locs, obstacle_locs, obstacle_radius
            )
            if earliest_collision_index > first_collision_index:
                earliest_collision_index = first_collision_index
                earliest_collision_loc = first_collision_loc
                if DEBUG:
                    print(
                        f"Possible collision obstacle id is {obstacle.id}, type is {obstacle.type_id}."
                    )
        time_to_crash = earliest_collision_index * PREDICTION_STEP
        if DEBUG:
            print(f"{time_to_crash} secs to collide at {earliest_collision_loc}!")
        yield_ind_in_wpts, yield_loc = OVERSIZE_INT * 10, None
        if earliest_collision_loc is not None:
            yield_ind_in_wpts, yield_loc = closest_vertex(
                self.waypoints, np.array([earliest_collision_loc])
            )

        # Determine if stop at traffic light
        stop_ind = OVERSIZE_INT * 10
        stop_loc = None
        if stop_at_junction:
            for ind, loc in enumerate(self.waypoints):
                # TODO: it might cause a problem that, the waypoints cross the intersection and stop outside the intersection; and the location is at the center of the car, the distance might not accurate
                dis_to_intersection = self.distance_to_intersection(
                    self.hd_map.get_waypoint(
                        Location(float(loc[0]), float(loc[1]), float(loc[2]))
                    )
                )
                if (
                    dis_to_intersection is not None
                    and 0.1 < dis_to_intersection < STOP_DIS_TO_INTERSECTION
                ):  # able to stop, and not in the intersection
                    stop_ind = ind
                    stop_loc = loc
                    break
        if stop_loc is None:
            if DEBUG:
                print(f"No stop at traffic light! Keep moving")

        truncate_index = min(yield_ind_in_wpts, stop_ind)
        if (
            truncate_index > OVERSIZE_INT
        ):  # No influence on the current local waypoints, keep moving
            self.decision.action = Action.NONE
            self.counter_from_stop += 1
        elif (
            time_to_crash <= EMERGENCY_STOP_TIME
            or np.linalg.norm(
                self.waypoints[truncate_index][:2] - self.cur_position[:2]
            )
            <= EMERGENCY_STOP_DIS
        ):
            self.decision.action = Action.STOP
            self.counter_from_stop = 0
        else:
            self.decision.action = Action.YIELD
            self.counter_from_stop += 1

        if self.counter_from_stop <= STOP_KEEPING_THRESHOLD:
            self.decision.action = Action.STOP

        if self.decision.action != Action.NONE:
            if self.decision.action == Action.STOP:
                truncate_index = 0
            truncate_index = max(2, truncate_index)
            self.waypoints = self.waypoints[:truncate_index]
            target_speed = 0
            cur_speed = np.linalg.norm(self.cur_speed)
            self.target_speeds = np.linspace(
                cur_speed, target_speed, len(self.waypoints)
            )

    def global_planning(self):
        if self.decision.status == Status.LANE_BORROW:
            self.plan_lane_borrow_traj()
        elif self.decision.status == Status.CRUISE:
            self.plan_cruise_traj()
        else:
            raise Exception(f"Status {self.decision.status} not implemented yet!")
        self.plan_yielding_traj()

    def update(
        self,
        position,
        speed,
        gps_waypoints,
        local_waypoints,
        local_trajectory,
        obstacles,
        ego_vehicle,
        other_obstacles,
    ):
        """
        For grount truth, obstacles are in (actor_id(str), actor_loc(np.array([x,y,z])), actor_velocity(np.array([x,y,z])), actor_bbox, transform), using ego_vehicle and other_obstacles. Otherwise, obstacles are in (actor_id(str), actor_loc(np.array([x,y,z])), actor_velocity(np.array([x,y,z]))), using obstacles.
        """
        self.gps_waypoints = gps_waypoints[:, :3]
        if DEBUG_GT:
            # Ego vehicle is the carla actor of ego vehicle
            # other_obstacles are in (actor_id(str), actor_loc(np.array([x,y,z])), actor_velocity(np.array([x,y,z])), actor_bbox))
            # All of them are in world frame
            ego_position = ego_vehicle.get_location()
            ego_speed = ego_vehicle.get_velocity()
            self.cur_position = np.array(
                [ego_position.x, ego_position.y, ego_position.z]
            )
            self.cur_speed = np.array([ego_speed.x, ego_speed.y, ego_speed.z])
            self.obstacles = other_obstacles
            if ego_vehicle.is_at_traffic_light():
                self.traffic_light = ego_vehicle.get_traffic_light()
        else:
            self.cur_position = position[:3]
            yaw = position[3]
            self.cur_speed = np.array([speed*np.cos(yaw), speed*np.sin(yaw), 0.0])
            self.obstacles = obstacles
            if DEBUG_PREDICTION:
                if self.obstacles:
                    for obs in self.obstacles:
                        print(f"Obstacle {obs.id} is at {obs.loc} with speed {obs.vel}")
                else:
                    print("No obstacles!")
            self.traffic_light = None  # TODO: get from upper stream

        # Get closest route centerline waypoint
        (index, _) = closest_vertex(self.gps_waypoints, np.array([self.cur_position]))
        # TODO: give an option to input either the waypoint or trajectory
        if len(local_waypoints) <= 1:
            self.local_waypoints = np.column_stack((self.waypoints, self.target_speeds))
        else:
            self.local_waypoints = np.array(
                [[x, y, self.cur_position[2], speed] for x, y, speed in local_waypoints]
            )  # [x, y, z, speed]

        # Get current lane centerline waypoint
        self.cur_lane_wpt = self.hd_map.get_waypoint(
            Location(
                float(self.cur_position[0]),
                float(self.cur_position[1]),
                float(self.cur_position[2]),
            ),
            project_to_road=True,
            lane_type=LaneType.Driving,
        )

        # Get info of self lane and adjacent lane
        _x, _y, _z = self.gps_waypoints[index]
        self.self_lane_wpt = self.hd_map.get_waypoint(
            Location(float(_x), float(_y), float(_z)),
            project_to_road=True,
            lane_type=LaneType.Driving,
        )
        self.left_lane_wpt = self.self_lane_wpt.get_left_lane()
        self.right_lane_wpt = self.self_lane_wpt.get_right_lane()

        # Blocking obstacle should be within 20 m on the route (Assume gps waypoints interval 1m)
        global_wpts_forward_checking = self.gps_waypoints[
            index : index + LANE_BORROW_BLOCKING_CHECKING_DISTANCE
        ]  # [x, y, z]
        route_forword_center_wpts = [
            self.hd_map.get_waypoint(
                Location(float(wpt[0]), float(wpt[1]), float(wpt[2])),
                project_to_road=True,
                lane_type=LaneType.Driving,
            )
            for wpt in global_wpts_forward_checking
        ]  # Carla waypoint
        route_forword_center_xyz = np.array(
            [
                [
                    wpt.transform.location.x,
                    wpt.transform.location.y,
                    wpt.transform.location.z,
                ]
                for wpt in route_forword_center_wpts
            ]
        )  # np.array([x, y, z], ...)

        # Check for blocking obstacles, decide if lane borrow is necessary
        if self.check_blocking_route(route_forword_center_xyz):
            self.obstacle_blocking_counter += 1
            self.able_to_use_self_route_counter = 0
            if DEBUG:
                print(
                    f"Decision Making, obstacle is blocking! Blocking coutner {self.obstacle_blocking_counter}"
                )
            if self.check_lane_borrow_conditions():
                self.start_lane_borrow()  # update status, choose path to borrow
        else:
            if DEBUG:
                print(f"No obstacle blocking the route!")
            self.obstacle_blocking_counter = 0
            self.able_to_use_self_route_counter += 1
            if self.able_to_use_self_route_counter >= LANE_BORROW_DISABLE_COUNTER:
                self.end_lane_borrow()  # Update status

        # Check for overtaking or yielding
        # self.check_yielding_obstacles()

        # Global Planning based on status
        self.global_planning()  # Update status
        if DEBUG:
            print("*" * 100)
            print(
                f"Decision making cruise status: {self.decision.status}, borrow lane {self.decision.borrow_lane}, yielding status {self.decision.action}."
            )
            if len(self.target_speeds) > 0:
                print(f"Target speed {self.target_speeds[-1] }")
            else:
                print(f"No target speed provided!!")


class Operator:
    """
    Compute Global waypoints based on GPS waypoints and obstacles, given a target_speed and global lanes around the waypoints.
    """

    def __init__(self):
        self.position = np.array([])
        self.hd_map = None
        self.speed = None
        self.obstacles = np.array([])
        self.obstacles_prediction = np.array([])
        self.goal_location = np.array([])
        self.local_waypoints = np.array([])
        self.local_trajectory = np.array([])
        self.gps_waypoints = np.array([])
        self.waypoints_array = np.array([])  # Include locations and speed

        self.decision_maker = None

        self.ego_vehicle = None
        self.other_obstacles = list()

    def get_obstacle_prediction_res(self, obstacles_prediction):
        # obstacle prediction includes actor id, current position [x,y,z], and average velocity [x,y,z] at first three predictions
        obstacles_prediction_res = []
        for prediction in obstacles_prediction:
            actor_id = int(prediction["actor_id"])
            obstacles_prediction_pos = prediction["actor_pos"][0]
            obstacles_prediction_vel = np.average(prediction["actor_velocity"][:PREDICTION_VEL_AVERAGE_WINDOW], axis=0)
            obstacles_prediction_res.append(
                Obstacle(actor_id, obstacles_prediction_pos, obstacles_prediction_vel)
            )
        for obs in self.obstacles:
            x, y, z, confidence, label = obs
            if confidence > OBSTACLE_CONFIDENCE_THRESHOLD:
                obstacles_prediction_res.append(Obstacle(label, np.array([x,y,z]), np.array([0,0,0])))
        obstacles_prediction_res.sort(key=lambda x: distance_points(x.loc, self.position[:3]))
        return np.array(obstacles_prediction_res)

    def on_event(
        self,
        dora_event: dict,
        send_output: Callable[[str, bytes], None],
    ) -> DoraStatus:
        if dora_event["type"] == "INPUT":
            return self.on_input(dora_event, send_output)
        return DoraStatus.CONTINUE

    def on_input(
        self,
        dora_input: dict,
        send_output: Callable[[str, bytes], None],
    ):
        if "position" == dora_input["id"]:
            [x, y, z, rx, ry, rz, rw] = np.frombuffer(dora_input["data"], np.float32)
            [_, _, yaw] = R.from_quat([rx, ry, rz, rw]).as_euler("xyz", degrees=False)
            self.position = np.array([x, y, z, yaw])

            if DEBUG_GT:
                # get actors
                all_vehicle_actors = client.get_world().get_actors().filter("vehicle.*")
                # set ego car
                if self.ego_vehicle is None:
                    for actor in reversed(all_vehicle_actors):
                        if "vehicle.lincoln.mkz_2017" in actor.type_id:
                            # in case other vehicle has the same name as ego car
                            location = actor.get_location()
                            actor_to_gps_dist = np.hypot(location.x - x, location.y - y)
                            if actor_to_gps_dist <= 3.0:
                                self.ego_vehicle = actor
                                break

            return DoraStatus.CONTINUE

        if "speed" == dora_input["id"]:
            self.speed = np.frombuffer(dora_input["data"], np.float32)[0]
            return DoraStatus.CONTINUE

        if "opendrive" == dora_input["id"]:
            if self.hd_map is None:
                # Registering the map
                opendrive = dora_input["data"].decode()
                self.hd_map = carla.Map("map", opendrive)
            return DoraStatus.CONTINUE

        if "obstacles" == dora_input["id"]:
            self.obstacles = np.frombuffer(dora_input["data"], np.float32).reshape(
                (-1, 5)
            )  # (x, y, z, confidence, label)

            return DoraStatus.CONTINUE

        if "obstacles_prediction_res" == dora_input["id"]:
            if dora_input["data"] == b"" or len(self.position) == 0:
                return DoraStatus.CONTINUE

            if DEBUG_GT:
                # get actors
                all_vehicle_actors = client.get_world().get_actors().filter("vehicle.*")
                all_people_actors = client.get_world().get_actors().filter("walker.*")
                all_actors = list(all_vehicle_actors) + list(all_people_actors)

                max_distance = 50.0  # Only consider obstacles within 50 m
                max_obstacles = 10  # Only consider top 10 obstacles

                # set obstacles
                if self.ego_vehicle:
                    ego_location = self.ego_vehicle.get_location()
                    heap_obstacle = list()

                    for actor in all_actors:
                        if (self.ego_vehicle.id != actor.id) and actor.id:
                            location = actor.get_location()
                            distance_to_ego = np.hypot(
                                location.x - ego_location.x, location.y - ego_location.y
                            )
                            if distance_to_ego <= max_distance:
                                velocity = actor.get_velocity()
                                obstacle = Obstacle(
                                    actor.id,
                                    location,
                                    velocity,
                                    actor.type_id,
                                    actor.bounding_box,
                                    actor.get_transform(),
                                )
                                if len(heap_obstacle) < max_obstacles:
                                    heapq.heappush(
                                        heap_obstacle, (-distance_to_ego, obstacle)
                                    )
                                elif distance_to_ego < -heap_obstacle[0][0]:
                                    # If the actor is closer than the farthest in current top max_obstacles
                                    heapq.heappop(heap_obstacle)
                                    heapq.heappush(
                                        heap_obstacle, (-distance_to_ego, obstacle)
                                    )

                    self.other_obstacles = [
                        heapq.heappop(heap_obstacle)[1]
                        for _ in range(len(heap_obstacle))
                    ]
                    self.other_obstacles.reverse()
                else:
                    self.other_obstacles = list()
            else:
                obstacles_prediction_res = self.get_obstacle_prediction_res(pickle.loads(dora_input["data"]))
                # if len(obstacles_prediction_res) == 0:
                #     return DoraStatus.CONTINUE
                self.obstacles_prediction = obstacles_prediction_res
            return DoraStatus.CONTINUE

        if "objective_waypoints" == dora_input["id"]:
            objective_waypoints = np.frombuffer(dora_input["data"], np.float32).reshape(
                (-1, 3)
            )

            if self.hd_map is None or len(self.position) == 0:
                return DoraStatus.CONTINUE
            self.goal_location = objective_waypoints[0]
            return DoraStatus.CONTINUE

        if "waypoints" == dora_input["id"]:
            self.local_waypoints = np.frombuffer(
                dora_input["data"], np.float32
            ).reshape((-1, 3))
            return DoraStatus.CONTINUE

        if "trajectory" == dora_input["id"]:
            self.local_trajectory = np.frombuffer(
                dora_input["data"], np.float32
            ).reshape((-1, 7))
            return DoraStatus.CONTINUE

        if "gps_waypoints" == dora_input["id"]:
            self.gps_waypoints = np.frombuffer(dora_input["data"], np.float32).reshape(
                (-1, 4)
            )

            if (
                self.hd_map is None
                or len(self.position) == 0
                or self.speed is None
                or len(self.gps_waypoints) == 0
                or len(self.goal_location) == 0
            ):
                return DoraStatus.CONTINUE

            if self.decision_maker is None:
                self.decision_maker = DecisionMaker(
                    self.hd_map, self.gps_waypoints, self.goal_location
                )

            if DEBUG_GT:
                if self.ego_vehicle is None:
                    return DoraStatus.CONTINUE

            self.decision_maker.update(
                self.position,
                self.speed,
                self.gps_waypoints,
                self.local_waypoints,
                self.local_trajectory,
                self.obstacles_prediction,
                self.ego_vehicle,
                self.other_obstacles,
            )

            if len(self.decision_maker.waypoints) == 0:
                send_output(
                    "global_waypoints",
                    self.decision_maker.waypoints.tobytes(),
                    {},
                )  # World coordinate

                send_output(
                    "decision_making",
                    Decision(Status.CRUISE, BorrowLane.NONE, Action.STOP).to_bytes(),
                    {},
                )
                return DoraStatus.CONTINUE

            self.waypoints_array = np.concatenate(
                [
                    self.decision_maker.waypoints.T,
                    self.decision_maker.target_speeds.reshape(1, -1),
                ]
            ).T.astype(np.float32)

            send_output(
                "global_waypoints",
                self.waypoints_array.tobytes(),  # [x, y, z, target_speed]
                {},
            )  # World coordinate

            send_output(
                "decision_making",
                self.decision_maker.decision.to_bytes(),
                {},
            )  # decision making status

            return DoraStatus.CONTINUE
